//
// Created by ze on 2023/11/2.
//

#ifndef ENCODER_UPRE_INT_H
#define ENCODER_UPRE_INT_H
#include "gpio.h"
#include "usart.h"
#include "math.h"
#include "stdio.h"
#include "stdint.h"
#include "stdbool.h"
#include "string.h"
#include "stdlib.h"//要使用malloc是要包含此头文件
#include "memory.h"//要使用memset是要包含此头文件
#include "stm32f4xx_hal.h"
#define PI 3.1415926535
//#define BlueTeeth
#define MODE 1  //0:舵轮  1:全向轮  2:麦轮
/******************陀螺仪选择*************************/
//#define GYRO_MINS500 1
//#define GYRO_100A    2
//#define GYRO_GI410    3
//#define GYRO_MODEL 1 //修改使用陀螺仪的型号
/******************打印开关*********************/
#define Printf_Pose             0      //打印机器人当前位姿
#define Printf_Gyro             0      //打印陀螺仪数据
#define Printf_Percent          0      //打印直线路径百分比
#define Printf_Percent_Bezier   0      //打印贝塞尔曲线百分比
#define Printf_MotorSpeed       0      //打印电机输出速度
#define Printf_TargetVel        0      //打印路径输出速度
#define Printf_BPointNow        0      //打印B样条当前点和百分比
#define Printf_BPoint           0      //打印B样条路径点
#define Printf_PathState        0      //路径状态 GOING = 20  FINISH = 21  NEW_PATH = 22  PATH_ERROR = 23; BSPLINE = 30 , STRAIGHT_LINE = 31 , FITTING_LINE = 32 , SPECIAL_STRAIGHT_LINE = 33;
#define Printf_Motor_Interface 0    //打印MOtor_Interface
//机器人位姿
typedef struct _Pose {
    float wx;
    float wy;
    float yaw;
}Pose;
typedef struct _Point
{
    float x;
    float y;
}point;
typedef struct _vector
{
    float size;
    float dir;
}vector;
typedef struct _WheelParam {
    const float RotationRadius;         //轮子半径
    const float PositiveDir;            //自旋时轮子的固定角度
    float currentPos;                   //舵电机转过的总角度
    vector targetVel;
} WheelParam;

typedef struct _WheelSys {
    WheelParam LeftFront;
    WheelParam RightFront;
    WheelParam RightRear;
    WheelParam LeftRear;
} WheelSys;

typedef struct _motor_measure_t {
    float dir;
    float last_dir;
    float total_dir;              //总转过的角度
}motor_measure;

typedef struct _WHEEL_Measure {
    motor_measure Wheel1_Measure;
    motor_measure Wheel2_Measure;
    motor_measure Wheel3_Measure;
    motor_measure Wheel4_Measure;
}Wheel_Name_Measure;

typedef struct _ChassisMovement {
    float omega;
    vector transVel;
}ChassisMovement;


/***************************************全场定位******************************************/


/****************************************路径*********************************************/
// 全场路径状态标志位
typedef enum PATH_STATE
{
    GOING = 20,
    FINISH,
    NEW_PATH,
    PATH_ERROR,
    STOP,
} PATH_STATE;
//路径样式
typedef enum PATH_TYPE
{
    BSPLINE = 30,
    STRAIGHT_LINE,
    FITTING_LINE,
    SPECIAL_STRAIGHT_LINE,
    BESSEL_LINE,
} PATH_TYPE;
typedef struct PathParam
{
    uint8_t pathType;
    uint8_t pathFinishDisp;
    uint8_t acceleratePercent;
    uint8_t descentPercent;
    uint8_t yawAdjustPercent;
    int32_t startSpeed;
    int32_t maxSpeed;
    int32_t endSpeed;
    int32_t decelerationDistance;
    Pose startPose;
    Pose midPose;
    Pose endPose;
    float Kp;
    float Ki;
    float Kd;
}PathParam;
typedef struct StraightLinePath
{
    Pose startPose;
    Pose midPose;
    Pose endPose;
}StraightLinePath;
typedef struct B_Spline_PathCfg
{
    float *knots;
    Pose *Control_Point;
    uint8_t k;
    uint8_t n;

}B_Spline_PathCfg;
typedef struct PATH_BesselControlPoint
{
    Pose	Path_BesselPoint_1;			//起点
    point	Path_BesselPoint_2;			//控制点1
    point	Path_BesselPoint_3;			//控制点2
    Pose	Path_BesselPoint_4;			//终点
    point	Path_BesselTransitionPoint[5];	//过度点
}PATH_BesselControlPoint;

/************************************pid****************************************/
typedef struct PIDInfo {
    float target;
    float kp;
    float ki;
    float kd;
    float ti;
    float td;

    float ka;
    float kb;
    float kc;

    float err;
    float last_err;
    float lalast_err;

    float output;
    float last_output;

    float limit_Integral;
    float Integral;

    float MaxOutput;
    float T;
    float Un;

    bool pid_MODE;// 1 增量式 0 位置式
}PIDInfo;

typedef struct PoseRegulator {
    PIDInfo xRegulator;
    PIDInfo yRegulator;
    PIDInfo yawRegulator;
}PoseRegulator;

typedef enum PidCalMode {
    PID_OFF,
    PID_X_Y_ONLY,
    PID_YAW_ONLY,
    PID_X_Y_YAW,
}PidCalMode;
//平方
static inline float square(float x)
{
    return (x * x);
}
//角度制转弧度制
static inline float Angle2Radian(float Angle)
{
    return (Angle * 0.0174532925f);
}
//弧度制转角度制
static inline float Radian2Angle(float Radian)
{
    return (Radian * 57.29577951f);
}
//精度限制在小数点后2位
static inline float To2DecimalPlaces(float src)
{
    return (float)(int)(src*100) / 100;
}
//精度限制在小数点后3位
static inline float To3DecimalPlaces(float src)
{
    return (float)(int)(src * 1000) / 1000;
}
//限制最大值
static inline float Limit_Max(float value, float Max)
{
    return (value > Max ? Max : value);
}
//限制最小值
static inline float Limit_Min(float value, float Min)
{
    return (value < Min ? Min : value);
}

// 将角度限制在 -180 ~ 180 之间
static inline float AngleLimit180(float dir)
{
    return(fabsf(dir) > 180.f ? (dir - 360 * ( (int32_t)(dir * 0.005556f) - (int32_t)(dir * 0.00277f) ) ) : dir);
}
// 将角度限制在 -360 ~ 360 之间
static inline float AngleLimit360(float dir)
{
    return(fabsf(dir) > 360.f ? dir - 360 *(int32_t)(dir * 0.002778f) : dir);
}

//将目标角度转化成与当前总角度同一周期的角度
static inline float Angle2SameCycle180(float srcCycledir, float destCycledir)
{
    return (AngleLimit180(srcCycledir) + ((int)(destCycledir * 0.005556f) - (int)(destCycledir * 0.00277f) ) * 360);
}
//将目标角度转化成与当前总角度同一周期的角度
static inline float Angle2SameCycle360(float srcCycledir, float destCycledir)
{
    return (AngleLimit360(srcCycledir) + ((int)(destCycledir * 0.002778f)) * 360.f);
}
// 矢量合成
static inline vector vectorSynthesis(const vector* vel1, const vector* vel2)
{
    vector SynthesisVel = { 0 };

    if (vel1 != NULL && vel2 != NULL) {
        float a = 0;
        float synthesisVelX = (vel1->size) * cosf(Angle2Radian(vel1->dir)) + (vel2->size) * cosf(Angle2Radian(vel2->dir));
        float synthesisVelY = (vel1->size) * sinf(Angle2Radian(vel1->dir)) + (vel2->size) * sinf(Angle2Radian(vel2->dir));


        SynthesisVel.size = sqrt(square(synthesisVelX) + square(synthesisVelY));

        a = atan2f(synthesisVelY, synthesisVelX);
        SynthesisVel.dir = Radian2Angle(a);

    }
    return SynthesisVel;
}
/*******************************************函数声明***************************************/
void Robot_Init();
/******************************************************************************************/

#endif //ENCODER_UPRE_INT_H
